#include "CollisionDetector.h"
#include "..\utils\Log.h"

const uint32 MAX_OBJECTS = 2048;

namespace ds {

// ----------------------------------------------------
// CollisionBuffer
// ----------------------------------------------------

CollisionBuffer::CollisionBuffer() {
    m_Buffer = new CharBuffer(1024);
}

CollisionBuffer::~CollisionBuffer() {
    delete m_Buffer;            
}

void CollisionBuffer::add(CDHandle firstHandle,uint32 firstID,const Vec2& firstPos,CDHandle secondHandle,uint32 secondID,const Vec2& secondPos) {
	bool found = false;
	CollisionEvent ce;
	for ( uint32 s = 0; s < m_Buffer->size(); ++s ) {
		m_Buffer->read(&ce,sizeof(CollisionEvent),s);
		if ( collision::containsID(ce,firstID) && collision::containsID(ce,secondID) ) {
			found = true;
		}
	}
	if ( !found ) {
		//LOG(logINFO) << "adding collision between " << firstID << " and " << secondID;
		CollisionEvent col;
		col.firstHandle = firstHandle;
		col.secondHandle = secondHandle;
		col.firstID = firstID;
		col.secondID = secondID;
		col.firstPos = firstPos;
		col.secondPos = secondPos;
		m_Buffer->write(&col,sizeof(CollisionEvent));
	}
}

void CollisionBuffer::get(uint32 index, CollisionEvent* collision) {
    m_Buffer->read(collision,sizeof(CollisionEvent),index);
}
// ----------------------------------------------------
//  Collision detector
// ----------------------------------------------------
CollisionDetector::CollisionDetector() : m_Index(0) {
    m_Positions = new Vec2[MAX_OBJECTS];
	m_Offsets = new Vec2[MAX_OBJECTS];
    m_Extents = new Vec2[MAX_OBJECTS];
    m_States = new bool[MAX_OBJECTS];
    m_Identifiers = new uint32[MAX_OBJECTS];
	m_Types = new uint32[MAX_OBJECTS];
    m_CB = new CollisionBuffer();
}

CollisionDetector::~CollisionDetector() {
	//LOG(logINFO) << "DELETING CD !!!!!";
    delete m_CB;	
	delete m_Types;
    delete m_Identifiers;
    delete m_States;
    delete m_Extents;
	delete m_Offsets;
    delete m_Positions;
}

void CollisionDetector::remove(CDHandle handle) {
	//LOG(logINFO) << "removing handle " << handle;
    m_States[handle] = false;
	m_FreeHandles.push_back(handle);
}

CDHandle CollisionDetector::addBox(const Vec2& position,const Vec2& extension,uint32 id) {
	return add(position,Vec2(0,0),extension,id,1);    
}

CDHandle CollisionDetector::addBox(const Vec2& position,const Vec2& offset,const Vec2& extension,uint32 id) {
	return add(position,offset,extension,id,1);    
}

CDHandle CollisionDetector::addCircle(const Vec2& position,float radius,uint32 id) {
	return add(position,Vec2(0,0),Vec2(radius,0.0f),id,2);	
}

CDHandle CollisionDetector::addCircle(const Vec2& position,const Vec2& offset,float radius,uint32 id) {
	return add(position,offset,Vec2(radius,0.0f),id,2);	
}

CDHandle CollisionDetector::add(const Vec2& position,const Vec2& offset,const Vec2& extent,uint32 id,uint32 type) {
	uint32 index = m_Index;
	if ( m_FreeHandles.size() > 0 ) {		
		CDHandle handle = m_FreeHandles[0];
		m_FreeHandles.erase(m_FreeHandles.begin());
		index = handle;
	}	
	else {
		++m_Index;
	}
	//LOG(logINFO) << "adding id: " << id << " type: " << type << " at index " << index;
	m_Positions[index] = position;
	m_Offsets[index] = offset;
	m_Extents[index] = extent;
	m_States[index] = true;
	m_Identifiers[index] = id;
	m_Types[index] = type;
	return index; 
}

void CollisionDetector::update() {    
    Vec2* pos = m_Positions;
	Vec2* offset = m_Offsets;
    Vec2* extent = m_Extents;
    bool* state = m_States;
	uint32* type = m_Types;
    uint32* id = m_Identifiers;
    for ( uint32 i = 0; i < m_Index; ++i ) {
        if ( *state == true ) {
            intersects(*pos,*offset,*extent,i,*id,*type);            
        }
        ++pos;
        ++extent;
        ++state;
        ++id;
		++type;
		++offset;
    }
}

void CollisionDetector::processBuffer() {
    if ( m_CB->size() > 0 ) {
        for ( size_t i = 0; i < m_Handlers.size(); ++i ) {
            CollisionHandler* handler = m_Handlers[i];
            handler->processBuffer(*m_CB);                
        }
    }
}

void CollisionDetector::intersects(const Vec2& pos,const Vec2& offset,const Vec2& ext,uint32 handle,uint32 identifier,uint32 type) const {
    Vec2* position = m_Positions;
    Vec2* extent = m_Extents;
	Vec2* offs = m_Offsets;
    bool* state = m_States;
	uint32* tp = m_Types;
    uint32* id = m_Identifiers;
    for ( uint32 i = 0; i < m_Index; ++i ) {
        if ( identifier != *id && *state == true ) {
			// box to box intersection
			if ( type == 1 && *tp == 1 ) {
				if ( checkBoxIntersection(pos,ext.x,ext.y,*position,extent->x,extent->y)) {  
					m_CB->add(handle,identifier,pos,i,*id,*position);                
				}
			}
			// circle to circle intersection
			if ( type == 2 && *tp == 2 ) {
				if ( checkCircle(pos,offset,ext.x,*position,*offs,extent->x) ) {  					
					m_CB->add(handle,identifier,pos,i,*id,*position);                
				}
			}
        }
        ++position;
        ++extent;
        ++state;
        ++id;
		++tp;
		++offs;
    }        
}

bool CollisionDetector::checkCircle(const Vec2& p1, const Vec2& o1, float r1, const Vec2& p2, const Vec2& o2, float r2) const {
	float dx = ( p1.x + o1.x ) - ( p2.x + o2.x );
	float dy = ( p1.y + o1.y ) - ( p2.y + o2.y );
	if ( ( dx*dx + dy*dy ) <= (( r1+r2) * (r1+r2)) ) {
		return true;
	}
	return false;
}

bool CollisionDetector::checkBoxIntersection(const Vec2& p1,float w1,float h1,const Vec2& p2,float w2,float h2) const {    
    float left1 = p1.x;
    float left2 = p2.x;
    float right1 = left1 + w1;
    float right2 = left2 + w2;
    float top1 = p1.y;
    float top2 = p2.y;
    float bottom1 = top1 + h1;
    float bottom2 = top2 + h2;
    if ( bottom1 < top2 ) {
        return false;
    }
    if ( top1 > bottom2 ) {
        return false;
    }
    if (right1 < left2) {
        return false;
    }
    if (left1 > right2) {
        return false;
    }
    return true;

}

}
